Implementieren Sie die Monte-Carlo-Lokalisation aus dem Skript (Seite 21ff.). Führen Sie hierzu einen künstlichen Fehler ein. Verwenden Sie ausserdem den differentiellen Roboter aus dem Robosim-Beispiel. Die Datei "data/multigausstest.tcl" enthält ein Code-Beispiel für einen Zufallsgenerator mit multivariater Gaussverteilung.
Hier wird der Algorithmus vorgestellt so wie er in der Vorlesung und im Programm implementiert wurde. Es ist dabei zu beachten, dass die folgenden Schritte nur durchgeführt werden, wenn eine Sensormessung gemacht wird. Dies ist der Fall wenn zum Beispiel nur alle 500 Schritte ein Update gemacht wird und dazwischen die Odometrie bemüht wird. Für die restlichen 499 Schritte wird nur das Odometriemodell aus Schritt 2 angewandt auf die Partikel angewandt, ohne Rauschen und ohne Sampling.
Es werden $K$ Partikel auf der Karte $\mathcal{W}$ gestreut, wobei jedes Partikel $p_k =(\mathbf{X}_k , w_k )$ aus einer Position $\mathbf{X}_k =(X_k ,Y_k ,\theta_k )^{\text{T}}$ und dem importance-factor $w_k$ besteht. Die drei verschiedenen Initialisierungen der Partikel können angedacht werden:
Für den Sampling-Schritt wird eine neue Generation an Partikeln $p'_k$ aus der Verteilung der alten Partikel gezogen, wobei Mehrfachziehungen erlaubt sind ($K$-mal ziehen mit Zurücklegen).
Es muss eine neue Position $\mathbf{X}'_k \sim P(l_t | a_t , l_{t-1})$ bestimmt werden.
Hierfür wird jeder Partikel $p'_k$ entsprechend der Steuersignale $(v_l , v_r)$ fortbewegt:
\begin{align} \mathbf{X}'_{k,\text{Odo.}} = \begin{pmatrix} X'_k\\ Y'_k\\ \theta'_k \end{pmatrix} + \begin{pmatrix} \frac{s_{L} + s_{R} }{2} \cos \left( \theta'_k + \frac{s_{R} -s_{L}}{2b}\right)\\ \frac{s_{L} + s_{R} }{2} \sin \left( \theta'_k + \frac{s_{R} -s_{L}}{2b}\right)\\ \frac{s_{R} - s_{L} }{b} \end{pmatrix} \text{ mit } s_{L}=v_{L}\Delta t \text{ und } s_{R}=v_{R}\Delta t \end{align}Nach dem Odometrie-Schritt wird jeder Partikel an der neuen Position verrauscht. Hierfür wird die Anfangsunsicherheit $\mathbf{C}_{\text{P},0}$ und die Fehlerfortpflanzung im aktuellen Schritt verwendet.
\begin{align} & \mathbf{X}'_{k,\text{Odo.+MC}} = \mathbf{X}'_k \sim \mathcal{N}( \mathbf{X}'_{k,\text{Odo.}} , \mathbf{F}_{\text{P}} \mathbf{C}_{\text{P},0} \mathbf{F}_{\text{P}}^{\text{T}} + \mathbf{F}_{\text{S}} \mathbf{C}_{\text{S}} \mathbf{F}_{\text{S}}^{\text{T}}) \end{align}Für jedes Partikel $p'_k$ an der neuen Position $\mathbf{X}'_{k,\text{Odo.+MC}}$ wird nun der neue Importance-Factor $w'_k$ anhand der Sensormessung $z_k$, und der Messgleichung $P(z_k |\mathbf{X}'_{k,\text{Odo.+MC}})$ bestimmt. Für diese Messung muss für jedes Partikel eine hypothetisierte Sensormessung durchgeführt werden (ein Laserscan an der Position $\mathbf{X}'_{k,\text{Odo.+MC}}$ des Partikels $p'_k$). Für die Unsicherheit zwischen der Messung $z_k$ eines Partikels und der realen Messung $z$ des Laserscanners wird eine Gaussche Verteilung an der Stelle $z$ mit einer festen Unsicherheit $\sigma_z$ ausgewertet:
\begin{align} & \tilde{w}_{k}=\mathcal{N}(z,z_k,\sigma_z) \end{align}Da ein Laserscan an der aktuellen Position nicht nur aus einem Messwert $z$, sondern aus $R$ Messwerten $z_r$ besteht, schreibt sich der neue unnormalisierte Importance-Factor wie folgt:
\begin{align} & \tilde{w}_{k}=\prod_{r=1}^R \mathcal{N}(z_r,z_{k,r},\sigma_z) \end{align}Zuletzt wird der normalisierte Importance-Factor berechnet:
\begin{align} & w'_{k}=\tilde{w}_{k} / \sum_{k=1}^K \tilde{w}_{k} \end{align}Durch die Berechnung von $\tilde{w}_k$ über alle Sensormessungen $R$ wird das Ergebnis numerisch instabil. Aus diesem Grund werden die Berechnungen in der $\log$-Domaine durchgeführt:
Für die Normalisierung wird die $\operatorname{logadd}$-Funktion für zwei Zahlen $a$ und $b$ mit $b < a$ wie folgt definiert:
Mit $\operatorname{logadd}(a,b,c)=\operatorname{logadd}(\operatorname{logadd}(a,b),c)$ lässt sich somit der unnormalisierte Importance-Factor leicht berechnen:
Setze $p_k = \left( \mathbf{X}'_{k,\text{Odo.+MC}}, w'_k\right)$ und starte wieder im Schritt 3, nachdem der Roboter wieder einen Odometrieschritt gemacht hat.